package pack.model;

import pack.help.Command;
import pack.help.RobotCommand;
import gui.Main;

public class Master implements Runnable{

	volatile public static int sleepfor;
	
	
	
	@Override
	// run function for runnable interface
	public void run() {
		System.out.println("enter");
		long timeStart = System.currentTimeMillis();
		int numberOfReady = 0;
		int numberOfCmd = 0;
		boolean pingReadyReceived = false;
		// initiate a ping to determine if uC is ready
		Main.getPort().write("ping");
		// the next command in line
		Command tmp = null;
		
		while(Main.isRunning()){
			Main.setWorking(true);
			Main.getPort().waitForReady();
			if (!pingReadyReceived){
				pingReadyReceived = true;
			} else {
				numberOfReady++;
				if (numberOfReady > 1){
					Main.shiftBuffer();
				}
			}
			// if the thread is stopped
			if (!Main.isRunning()) break;
			tmp = Main.readRow();
			
			if (tmp == null) { 
				sleepfor = 0;
				if (numberOfReady-1 == numberOfCmd || numberOfCmd == 0)
					break; 				
			} 
			if (tmp != null) sleepfor = tmp.getTime();
			if (Main.getPort().getOnline()) {
				if (tmp != null) {
					Main.getControl().move(tmp.getTime(),tmp.getV1(),tmp.getV2()); 
					numberOfCmd++;
				}
			} else {
				
				float stepDistanceM = RealLifeProperties.StepDistanceMM/1000;
				int timerFrequency = RealLifeProperties.timerFrequency;
				float v1 = tmp.getV1();
				float v2 = tmp.getV2();
				float timeS = tmp.getTime()/1000;
				RobotCommand tmpRCmd = new RobotCommand(new Command(tmp.getTime(),v1,v2));
				Main.prevSpL = tmpRCmd.getSpeedL();
				Main.prevSpR = tmpRCmd.getSpeedR();
				
				String sp1 = Control.intToString(tmpRCmd.getSpeedR(),3);
				String sp2 = Control.intToString(tmpRCmd.getSpeedL(),3);
				String st1 = Control.intToString(tmpRCmd.getStepsR(),4);
				String st2 = Control.intToString(tmpRCmd.getStepsL(),4);
							
				
				System.out.println("run " + "ff" + " " +sp1+" "+sp2+" "+st1+" "+st2+" ");
				
				
				System.out.println("Moving for " + tmp.getTime() + "ms, with v1= " + 
						tmp.getV1() + "m/s and v2=" + tmp.getV2() + "m/s.");
				Main.getSimulator().newCommand(tmp);
			}
			tmp = null;
		}
		Main.done();
		System.out.println("Time spent: " + Long.toString(System.currentTimeMillis()-timeStart) + "ms");
		System.out.println("exit");
		
	}	
	
}
